#include "arm_solution.h"
#include "matrix.h"
#include "math.h"

#define PI 3.1415926535897932384626433832795

/**
 * @brief 范围限幅
 * @note  输入六轴机械臂的位姿(前三个元素为位置，后三个元素ZYZ顺次欧拉角)，判断是否超限,超限返回0，否则返回1
 * @param range ：机械臂位姿
 * @param numR ：数组元素个数
 */
int RangeLimit(float* range ,int numR)
{
  if(pow(RANGE_MAX,2) <= pow(range[0],2) +pow(range[1],2) +pow(range[2],2)) return 0;
  if(pow(RANGE_MIN,2) >= pow(range[0],2) +pow(range[1],2) +pow(range[2],2)) return 0;
  return 1;
}

/**
 * @brief 角度限幅
 * @note  输入六轴机械臂的六轴转角，判断是否超限
 * @param angle ：六轴转角
 * @param numA ：数组元素个数
 */
int AngleLimit(float* angle,int numA)
{
  //判断数据是否合法
  if(numA != 6) return 0;

  if(angle[0] >= DOF_MAX_ONE)   angle[0] = DOF_MAX_ONE;
  if(angle[0] <= DOF_MIN_ONE)   angle[0] = DOF_MIN_ONE;

  if(angle[1] >= DOF_MAX_TWO)   angle[1] = DOF_MAX_TWO;
  if(angle[1] <= DOF_MIN_TWO)   angle[1] = DOF_MIN_TWO;

  if(angle[2] >= DOF_MAX_THREE) angle[2] = DOF_MAX_THREE;
  if(angle[2] <= DOF_MIN_THREE) angle[2] = DOF_MIN_THREE;

  if(angle[3] >= DOF_MAX_FOUR)  angle[3] = DOF_MAX_FOUR;
  if(angle[3] <= DOF_MIN_FOUR)  angle[3] = DOF_MIN_FOUR;

  if(angle[4] >= DOF_MAX_FIVE)  angle[4] = DOF_MAX_FIVE;
  if(angle[4] <= DOF_MIN_FIVE)  angle[4] = DOF_MIN_FIVE;

  if(angle[5] >= DOF_MAX_SIX)   angle[5] = DOF_MAX_SIX;
  if(angle[5] <= DOF_MIN_SIX)   angle[5] = DOF_MIN_SIX;
  return 1;
}
/**
 * @brief 逆运动学解算
 * @note  输入一个位姿向量(前三个元素为位置，后三个元素为ZYZ顺次欧拉角)，得到六轴机械臂的六轴转角
 * @param Xik : 位姿向量
 * @param Jik ：六轴转角
 */
void InverseK(float* Xik, float* Jik)
{
  // inverse kinematics
  // input: Xik - pos value for the calculation of the inverse kinematics
  // output: Jfk - joints value for the calculation of the inversed kinematics
  
  // from deg to rad
  // Xik(4:6)=Xik(4:6)*pi/180;
  Xik[3]=Xik[3]*PI/180.0;
  Xik[4]=Xik[4]*PI/180.0;
  Xik[5]=Xik[5]*PI/180.0;
  
  // Denavit-Hartenberg matrix
  float theta[6]={0.0, -90.0, 0.0, 0.0, 0.0, 0.0}; // theta=[0; -90+0; 0; 0; 0; 0];
  float alfa[6]={-90.0, 0.0, -90.0, 90.0, -90.0, 0.0}; // alfa=[-90; 0; -90; 90; -90; 0];
  float r[6]={47.0, 110.0, 26.0, 0.0, 0.0, 0.0}; // r=[47; 110; 26; 0; 0; 0];
  float d[6]={133.0, 0.0, 7.0, 117.50, 0.0, 28.0}; // d=[133; 0; 7; 117.5; 0; 28];
  // from deg to rad
  MatrixScale(theta, 6, 1, PI/180.0, sizeof(theta)/sizeof(float)); // theta=theta*pi/180;
  MatrixScale(alfa,  6, 1, PI/180.0, sizeof(alfa)/sizeof(float)); // alfa=alfa*pi/180;
  
  // work frame
  float Xwf[6]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // Xwf=[0; 0; 0; 0; 0; 0];
  
  // tool frame
  float Xtf[6]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // Xtf=[0; 0; 0; 0; 0; 0];
  
  // work frame transformation matrix
  float Twf[16];
  pos2tran(Xwf, Twf, sizeof(Xwf)/sizeof(float), sizeof(Twf)/sizeof(float)); // Twf=pos2tran(Xwf);
  
  // tool frame transformation matrix
  float Ttf[16];
  pos2tran(Xtf, Ttf, sizeof(Xtf)/sizeof(float), sizeof(Ttf)/sizeof(float)); // Ttf=pos2tran(Xtf);
  
  // total transformation matrix
  // 注意Xik为指针，不能直接计算大小
  float Twt[16];
  pos2tran(Xik, Twt, 6, sizeof(Twf)/sizeof(float)); // Twt=pos2tran(Xik);
  
  // find T06
  float inTwf[16], inTtf[16], Tw6[16], T06[16];
  invtran(Twf, inTwf, sizeof(Twf)/sizeof(float), sizeof(inTwf)/sizeof(float)); // inTwf=invtran(Twf);
  invtran(Ttf, inTtf, sizeof(Ttf)/sizeof(float), sizeof(inTtf)/sizeof(float)); // inTtf=invtran(Ttf);
  MatrixMultiply(Twt, inTtf, 4, 4, 4, Tw6, sizeof(Twt)/sizeof(float), sizeof(inTtf)/sizeof(float), sizeof(Tw6)/sizeof(float)); // Tw6=Twt*inTtf;
  MatrixMultiply(inTwf, Tw6, 4, 4, 4, T06, sizeof(inTwf)/sizeof(float), sizeof(Tw6)/sizeof(float), sizeof(T06)/sizeof(float)); // T06=inTwf*Tw6;
  
  // positon of the spherical wrist
  float Xsw[3];
  // Xsw=T06(1:3,4)-d(6)*T06(1:3,3);
  Xsw[0]=T06[0*4 + 3]-d[5]*T06[0*4 + 2];
  Xsw[1]=T06[1*4 + 3]-d[5]*T06[1*4 + 2];
  Xsw[2]=T06[2*4 + 3]-d[5]*T06[2*4 + 2];
  
  // joints variable
  // Jik=zeros(6,1);
  // first joint
  //出现多解时取正号，后续可优化
  Jik[0]=atan2(Xsw[1],Xsw[0])-atan2(d[2],sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])); // Jik(1)=atan2(Xsw(2),Xsw(1))-atan2(d(3),sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2));
  // second joint
  Jik[1]=PI/2.0
  -acos((r[1]*r[1]+(Xsw[2]-d[0])*(Xsw[2]-d[0])+(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0])*(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0])-(r[2]*r[2]+d[3]*d[3]))/(2.0*r[1]*sqrt((Xsw[2]-d[0])*(Xsw[2]-d[0])+(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0])*(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0]))))
  -atan((Xsw[2]-d[0])/(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0])); // Jik(2)=pi/2-acos((r(2)^2+(Xsw(3)-d(1))^2+(sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1))^2-(r(3)^2+d(4)^2))/(2*r(2)*sqrt((Xsw(3)-d(1))^2+(sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1))^2)))-atan((Xsw(3)-d(1))/(sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1)));
  // third joint
  Jik[2]=PI
  -acos((r[1]*r[1]+r[2]*r[2]+d[3]*d[3]-(Xsw[2]-d[0])*(Xsw[2]-d[0])-(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0])*(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0]))/(2*r[1]*sqrt(r[2]*r[2]+d[3]*d[3])))
  -atan(d[3]/r[2]); // Jik(3)=pi-acos((r(2)^2+r(3)^2+d(4)^2-(Xsw(3)-d(1))^2-(sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1))^2)/(2*r(2)*sqrt(r(3)^2+d(4)^2)))-atan(d(4)/r(3));
  // last three joints
  float T01[16], T12[16], T23[16], T02[16], T03[16], inT03[16], T36[16];
  DH1line(theta[0]+Jik[0], alfa[0], r[0], d[0], T01, sizeof(T01)/sizeof(float)); // T01=DH1line(theta(1)+Jik(1),alfa(1),r(1),d(1));
  DH1line(theta[1]+Jik[1], alfa[1], r[1], d[1], T12, sizeof(T12)/sizeof(float)); // T12=DH1line(theta(2)+Jik(2),alfa(2),r(2),d(2));
  DH1line(theta[2]+Jik[2], alfa[2], r[2], d[2], T23, sizeof(T23)/sizeof(float)); // T23=DH1line(theta(3)+Jik(3),alfa(3),r(3),d(3));
  MatrixMultiply(T01, T12, 4, 4, 4, T02, sizeof(T01)/sizeof(float), sizeof(T12)/sizeof(float), sizeof(T02)/sizeof(float)); // T02=T01*T12;
  MatrixMultiply(T02, T23, 4, 4, 4, T03, sizeof(T02)/sizeof(float), sizeof(T23)/sizeof(float), sizeof(T03)/sizeof(float)); // T03=T02*T23;
  invtran(T03, inT03, sizeof(T03)/sizeof(float), sizeof(inT03)/sizeof(float)); // inT03=invtran(T03);
  MatrixMultiply(inT03, T06, 4, 4, 4, T36, sizeof(inT03)/sizeof(float), sizeof(T06)/sizeof(float), sizeof(T36)/sizeof(float)); // T36=inT03*T06;
  // forth joint
  Jik[3]=atan2(-T36[1*4+2], -T36[0*4+2]); // Jik(4)=atan2(-T36(2,3),-T36(1,3));
  // fifth joint
  Jik[4]=atan2(sqrt(T36[0*4+2]*T36[0*4+2]+T36[1*4+2]*T36[1*4+2]), T36[2*4+2]); // Jik(5)=atan2(sqrt(T36(1,3)^2+T36(2,3)^2),T36(3,3));
  // sixth joints
  Jik[5]=atan2(-T36[2*4+1], T36[2*4+0]); // Jik(6)=atan2(-T36(3,2),T36(3,1));
  // rad to deg
  MatrixScale(Jik, 6, 1, 180.0/PI, 6); // Jik=Jik/pi*180;
}

/**
 * @brief 正运动学解算
 * @note  输入六轴机械臂的六轴转角，得到一个位姿向量(前三个元素为位置，后三个元素为ZYZ顺次欧拉角)
 * @param Jfk : 六轴转角
 * @param Xfk ：位姿向量
 */
void ForwardK(float* Jfk, float* Xfk)
{
  // forward kinematics
  // input: Jfk - joints value for the calculation of the forward kinematics
  // output: Xfk - pos value for the calculation of the forward kinematics
  
  // Denavit-Hartenberg matrix
  float theTemp[6]={0.0, -90.0, 0.0, 0.0, 0.0, 0.0};
  float theta[6];
  MatrixAdd(theTemp, Jfk, 6, 1, theta, sizeof(theTemp)/sizeof(float), 6, sizeof(theta)/sizeof(float)); // theta=[Jfk(1); -90+Jfk(2); Jfk(3); Jfk(4); Jfk(5); Jfk(6)];
  float alfa[6]={-90.0, 0.0, -90.0, 90.0, -90.0, 0.0}; // alfa=[-90; 0; -90; 90; -90; 0];
  float r[6]={47.0, 110.0, 26.0, 0.0, 0.0, 0.0}; // r=[47; 110; 26; 0; 0; 0];
  float d[6]={133.0, 0.0, 7.0, 115.0, 0.0, 28.0}; // d=[133; 0; 7; 117.5; 0; 28];
  // from deg to rad
  MatrixScale(theta, 6, 1, PI/180.0, sizeof(theta)/sizeof(float)); // theta=theta*pi/180;
  MatrixScale(alfa, 6, 1, PI/180.0 , sizeof(alfa)/sizeof(float)); // alfa=alfa*pi/180;
  
  // work frame
  float Xwf[6]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // Xwf=[0; 0; 0; 0; 0; 0];
  
  // tool frame
  float Xtf[6]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // Xtf=[0; 0; 0; 0; 0; 0];
  
  // work frame transformation matrix
  float Twf[16];
  pos2tran(Xwf, Twf, sizeof(Xwf)/sizeof(float), sizeof(Twf)/sizeof(float)); // Twf=pos2tran(Xwf);
  
  // tool frame transformation matrix
  float Ttf[16];
  pos2tran(Xtf, Ttf, sizeof(Xtf)/sizeof(float), sizeof(Ttf)/sizeof(float)); // Ttf=pos2tran(Xtf);
  
  // DH homogeneous transformation matrix
  float T01[16], T12[16], T23[16], T34[16], T45[16], T56[16];
  DH1line(theta[0], alfa[0], r[0], d[0], T01, sizeof(T01)/sizeof(float)); // T01=DH1line(theta(1),alfa(1),r(1),d(1));
  DH1line(theta[1], alfa[1], r[1], d[1], T12, sizeof(T12)/sizeof(float)); // T12=DH1line(theta(2),alfa(2),r(2),d(2));
  DH1line(theta[2], alfa[2], r[2], d[2], T23, sizeof(T23)/sizeof(float)); // T23=DH1line(theta(3),alfa(3),r(3),d(3));
  DH1line(theta[3], alfa[3], r[3], d[3], T34, sizeof(T34)/sizeof(float)); // T34=DH1line(theta(4),alfa(4),r(4),d(4));
  DH1line(theta[4], alfa[4], r[4], d[4], T45, sizeof(T45)/sizeof(float)); // T45=DH1line(theta(5),alfa(5),r(5),d(5));
  DH1line(theta[5], alfa[5], r[5], d[5], T56, sizeof(T56)/sizeof(float)); // T56=DH1line(theta(6),alfa(6),r(6),d(6));

  float Tw1[16], Tw2[16], Tw3[16], Tw4[16], Tw5[16], Tw6[16], Twt[16];
  MatrixMultiply(Twf, T01, 4, 4, 4, Tw1, sizeof(Twf)/sizeof(float), sizeof(T01)/sizeof(float), sizeof(Tw1)/sizeof(float)); // Tw1=Twf*T01;
  MatrixMultiply(Tw1, T12, 4, 4, 4, Tw2, sizeof(Tw1)/sizeof(float), sizeof(T12)/sizeof(float), sizeof(Tw2)/sizeof(float)); // Tw2=Tw1*T12;
  MatrixMultiply(Tw2, T23, 4, 4, 4, Tw3, sizeof(Tw2)/sizeof(float), sizeof(T23)/sizeof(float), sizeof(Tw3)/sizeof(float)); // Tw3=Tw2*T23;
  MatrixMultiply(Tw3, T34, 4, 4, 4, Tw4, sizeof(Tw3)/sizeof(float), sizeof(T34)/sizeof(float), sizeof(Tw4)/sizeof(float)); // Tw4=Tw3*T34;
  MatrixMultiply(Tw4, T45, 4, 4, 4, Tw5, sizeof(Tw4)/sizeof(float), sizeof(T45)/sizeof(float), sizeof(Tw5)/sizeof(float)); // Tw5=Tw4*T45;
  MatrixMultiply(Tw5, T56, 4, 4, 4, Tw6, sizeof(Tw5)/sizeof(float), sizeof(T56)/sizeof(float), sizeof(Tw6)/sizeof(float)); // Tw6=Tw5*T56;
  MatrixMultiply(Tw6, Ttf, 4, 4, 4, Twt, sizeof(Tw6)/sizeof(float), sizeof(Ttf)/sizeof(float), sizeof(Twt)/sizeof(float)); // Twt=Tw6*Ttf;
  
  // calculate pos from transformation matrix
  tran2pos(Twt, Xfk, sizeof(Twt)/sizeof(float), 6); // Xfk=tran2pos(Twt);
  // Xfk(4:6)=Xfk(4:6)/pi*180;
  Xfk[3]=Xfk[3]/PI*180.0;
  Xfk[4]=Xfk[4]/PI*180.0;
  Xfk[5]=Xfk[5]/PI*180.0;
}
